#!/usr/bin/env python3
# zc4_moveit_config/launch/zc4_servo.launch.py

import launch
import launch_ros
from ament_index_python.packages import get_package_share_directory
from launch.substitutions import LaunchConfiguration
from launch.actions import DeclareLaunchArgument
from launch_param_builder import ParameterBuilder
from moveit_configs_utils import MoveItConfigsBuilder


def generate_launch_description():
    # 是否作为独立节点运行（这里给个开关，后面你想拓展 component 也方便）
    launch_as_standalone_node = LaunchConfiguration(
        "launch_as_standalone_node", default="true"
    )

    declare_standalone_arg = DeclareLaunchArgument(
        "launch_as_standalone_node",
        default_value="true",
        description="If true, launch moveit_servo as a standalone node.",
    )

    # 生成 MoveIt 相关参数
    # 注意 file_path 要和你 zc4_moveit_config 里面实际的一致
    moveit_config = (
        MoveItConfigsBuilder("zc4model", package_name="zc4_moveit_config")
        .robot_description(file_path="config/zc4model.urdf.xacro")
        .to_moveit_configs()
    )

    # Servo 参数（你的 zc4_servo.yaml）
    servo_params = {
        "moveit_servo": ParameterBuilder("zc4_moveit_config")
        .yaml("config/zc4_servo.yaml")
        .to_dict()
    }

    # 额外参数
    acceleration_filter_update_period = {"update_period": 0.01}
    planning_group_name = {"planning_group_name": "zc4"}

    # 独立 Servo 节点（最简单的写法）
    servo_node = launch_ros.actions.Node(
        package="moveit_servo",
        executable="servo_node",
        name="servo_node",
        output="screen",
        parameters=[
            servo_params,
            acceleration_filter_update_period,
            planning_group_name,
            moveit_config.robot_description,
            moveit_config.robot_description_semantic,
            moveit_config.robot_description_kinematics,
            moveit_config.joint_limits,
        ],
    )

    return launch.LaunchDescription(
        [
            declare_standalone_arg,
            servo_node,
        ]
    )
